/*-------------------------------motrotech----------------------------------
 * motrotech.cpp            
 * Copyright                motrotech
 * Response                 John Zhang
 * Description              Servo drive control
 * Version                  V6.0
 *---------------------------------------------------------------------------*/
//#define MTRACE
/*-INCLUDES------------------------------------------------------------------*/
#include <AtEthercat.h>
#include "motrotech.h"
#include "Logging.h"

/*-DEFINES-------------------------------------------------------------------*/
#define LogMsg      poLog_temp->LogMsg
#define LogError    poLog_temp->LogError
#define SLAVE_NOT_FOUND 0xFFFF

#define MBX_TIMEOUT 5000

#define MOTOR_TESTING

/*-GLOBAL VARIABLES-----------------------------------------------------------*/
My_Slave_Type My_Slave[MAX_SERVO_NUM];

/*-LOCAL VARIABLES-----------------------------------------------------------*/
static CAtEmLogging* poLog_temp;           /*for log print*/
static EC_T_INT  MotorCount = 0;

static EC_T_DWORD          S_ProcessState[6];
//static EC_T_WORD           ControlMode = 0;

/*-FUNCTION DEFINITIONS------------------------------------------------------*/

/******************************************************************************
Function: myInit
Description: Initialize variables
Called By: myAppInit
Return: EC_E_NOERROR
*******************************************************************************/
EC_T_DWORD myInit (CAtEmLogging* poLog)
{
    poLog_temp = poLog;

    OsMemset(My_Slave, 0, MAX_SERVO_NUM * sizeof(My_Slave_Type));
	for(EC_T_DWORD temp = 0; temp < 7; temp++) 
	    My_Slave[temp].wState = DRV_DEV_STATE_NOT_READY;
    //for(EC_T_DWORD temp = 0; temp < MAX_SERVO_NUM; temp++)
    //{
          //My_Slave[temp].ServoNum	= SLAVE_NOT_FOUND;
    //}

    return EC_E_NOERROR;
}

/******************************************************************************
Function?myPrepare
Description: search target slave PDO address; modify the My_Slave number, using S_dwAppFoundSlaves as the My_slave index
Called By: myAppPrepare
Return: EC_E_NOERROR
*******************************************************************************/
EC_T_DWORD myPrepare (
     CAtEmLogging*          poLog
    ,EC_T_DWORD             dwVendorId      /**< [in]       Vendor Id of slave to search */
    ,EC_T_DWORD             dwProductCode   /**< [in]       Product Code of slave to search */
    ,EC_T_DWORD*            slave_num       /**< [in/out]   S_dwAppFoundSlaves as input*/
)
{        
    EC_T_WORD wFixedAddress = 0;
	EC_T_WORD FoundFixedAddress =0;
	//EC_T_WORD SlaveNum = 0;

    EC_UNREFPARM(poLog);
    EC_UNREFPARM(dwVendorId);
    EC_UNREFPARM(dwProductCode);

    LogMsg("\n Motrotech: ___________________myprepare_______________________________");

    for (EC_T_DWORD i = 0; i < emGetNumConnectedSlaves(INSTANCE_MASTER_DEFAULT); i++)
    {
        if (FindSlaveGetFixedAddr(INSTANCE_MASTER_DEFAULT, poLog, i, dwVendorId, dwProductCode, &wFixedAddress))
        {
            if ((FoundFixedAddress != wFixedAddress) && (wFixedAddress >= 1001))
            {
                (*slave_num)++;
                MotorCount++;
				        FoundFixedAddress = wFixedAddress;

                    LogMsg("Motrotech: (%d)my drive have find",MotorCount);
//		        }
            }
        }
    }
    return EC_E_NOERROR;
}

/******************************************************************************
Function?mySetup
Description: Slave variable initialization; add index 0x6060; modify the scan into mysetup in next step
Called By: myAppSetup
Return: EC_E_NOERROR
*******************************************************************************/
EC_T_DWORD mySetup(
    CAtEmLogging*          poLog
    ,EC_T_BYTE*         pbyPDIn       /**< [in]  pointer to process data input buffer */
    ,EC_T_BYTE*         pbyPDOut      /**< [in]  pointer to process data output buffer */
)      
{   
    EC_T_DWORD  dwRetVal = EC_E_NOERROR;
    EC_T_WORD   wReadEntries = 0;
    EC_T_WORD   wWriteEntries = 0;
    EC_T_WORD   mySlaveOutVarInfoNum = 0;
    EC_T_WORD   mySlaveInVarInfoNum = 0;
    EC_T_PROCESS_VAR_INFO_EX* pProcessVarInfoOut = EC_NULL;
    EC_T_PROCESS_VAR_INFO_EX* pProcessVarInfoIn  = EC_NULL;

    EC_UNREFPARM(poLog);

    LogMsg("\n Motrotech: ___________________mysetup______________________");

    if ((pbyPDOut != EC_NULL) && (pbyPDIn != EC_NULL))
    {
		for (EC_T_INT MySlaves_num_tmp = 0; MySlaves_num_tmp < MotorCount; MySlaves_num_tmp++)
        {
			      if (ecatGetCfgSlaveInfo(EC_TRUE, (EC_T_WORD)(1001 + MySlaves_num_tmp), &My_Slave[MySlaves_num_tmp].S_aSlaveList) != EC_E_NOERROR)
            {
                LogError("ERROR: ecatGetCfgSlaveInfo() returns with error.");
				        continue;
            }
			      My_Slave[MySlaves_num_tmp].wFixedAddress = (EC_T_WORD)(1001 + MySlaves_num_tmp);

			/********************** PDO_OUT ***********************/
            mySlaveOutVarInfoNum = My_Slave[MySlaves_num_tmp].S_aSlaveList.wNumProcessVarsOutp;
            LogMsg("Motrotech: OurVarInfoNum = %d" ,mySlaveOutVarInfoNum);

            pProcessVarInfoOut = (EC_T_PROCESS_VAR_INFO_EX*)OsMalloc(sizeof(EC_T_PROCESS_VAR_INFO_EX) * mySlaveOutVarInfoNum);
            if(pProcessVarInfoOut == EC_NULL)
            {
                dwRetVal = EC_E_NOMEMORY;
                LogMsg("Motrotech: Malloc memory fail");
            }
            else
            {
				dwRetVal = ecatGetSlaveOutpVarInfoEx(EC_TRUE, My_Slave[MySlaves_num_tmp].wFixedAddress, mySlaveOutVarInfoNum, pProcessVarInfoOut, &wReadEntries);
				if (EC_E_NOERROR != dwRetVal)
                {
                    LogError( (EC_T_CHAR*)"ERROR: emGetSlaveInpVarInfoEx() (Result = %s 0x%x)", ecatGetText(dwRetVal), dwRetVal );
                }
            }

            for(int i = 0; i < mySlaveOutVarInfoNum; i++)
            {
                if(pProcessVarInfoOut[i].wIndex == DRV_OBJ_CONTROL_WORD)  // 0x6040 - ControlWord
                {
                    My_Slave[MySlaves_num_tmp].myControlWord  = (EC_T_WORD*)&(pbyPDOut[pProcessVarInfoOut[i].nBitOffs/8]);
                    LogMsg("Motrotech: My_Slave[%d].myControlWord = %d", MySlaves_num_tmp, My_Slave[MySlaves_num_tmp].myControlWord );
                }
                else if(pProcessVarInfoOut[i].wIndex == DRV_OBJ_TARGET_POSITION)  // 0x607A - TargetPosition
                {
                    My_Slave[MySlaves_num_tmp].myTargetPosition = (EC_T_DWORD*)&(pbyPDOut[pProcessVarInfoOut[i].nBitOffs/8]);
                    LogMsg("Motrotech: My_Slave[%d].myTargetPosition = %d", MySlaves_num_tmp, My_Slave[MySlaves_num_tmp].myTargetPosition );
                }
                else if(pProcessVarInfoOut[i].wIndex == DRV_OBJ_MODES_OF_OPERATION)   // 0x6060 - Mode of Operation
                {
                    My_Slave[MySlaves_num_tmp].myModeOfOperation = (EC_T_BYTE*)&(pbyPDOut[pProcessVarInfoOut[i].nBitOffs/8]);
                    LogMsg("Motrotech: My_Slave[%d].myModeOfOperation = %d", MySlaves_num_tmp, My_Slave[MySlaves_num_tmp].myModeOfOperation );
                }
//                else if(pProcessVarInfoOut[i].wIndex == DRV_OBJ_DIGITAL_OUTPUT)  // 0x7010 - Output 1
//                {
//                	  if(pProcessVarInfoOut[i].wSubIndex == DRV_OBJ_DIGITAL_OUTPUT_SUBINDEX_1)  // 0x7010/1 - Output subindex 1
//                    {
//                        My_Slave[MySlaves_num_tmp].myOutput_1 = (EC_T_WORD*)&(pbyPDOut[pProcessVarInfoOut[i].nBitOffs/8]);
//                        LogMsg("Motrotech: My_Slave[%d].myOutput_1 = %d", MySlaves_num_tmp, My_Slave[MySlaves_num_tmp].myOutput_1 );
//                    }
//                    else if(pProcessVarInfoOut[i].wSubIndex == DRV_OBJ_DIGITAL_OUTPUT_SUBINDEX_2)  // 0x7010/2 - Output subindex 2
//                    {
//                        My_Slave[MySlaves_num_tmp].myOutput_2 = (EC_T_WORD*)&(pbyPDOut[pProcessVarInfoOut[i].nBitOffs/8]);
//                        LogMsg("Motrotech: My_Slave[%d].myOutput_2 = %d", MySlaves_num_tmp, My_Slave[MySlaves_num_tmp].myOutput_2 );
//                    }
//                }
                else
                {
                	  //LogMsg("Motrotech: My_Slave[%d] Output undefine the index %d / usbindex %d", i, (pProcessVarInfoOut[i].wIndex, pProcessVarInfoOut[i].wSubIndex);
                }
            }

            /********************** PDO_IN ***********************/
            mySlaveInVarInfoNum = My_Slave[MySlaves_num_tmp].S_aSlaveList.wNumProcessVarsInp;
            LogMsg("Motrotech: InVarInfoNum = %d", mySlaveInVarInfoNum);

            pProcessVarInfoIn = (EC_T_PROCESS_VAR_INFO_EX*)OsMalloc(sizeof(EC_T_PROCESS_VAR_INFO_EX) * mySlaveInVarInfoNum);
            if(pProcessVarInfoIn == EC_NULL)
            {
                dwRetVal = EC_E_NOMEMORY;
                LogMsg("Motrotech: Malloc memory fail");
            }
            else
            {
                dwRetVal = ecatGetSlaveInpVarInfoEx(EC_TRUE, My_Slave[MySlaves_num_tmp].wFixedAddress, mySlaveInVarInfoNum, pProcessVarInfoIn, &wWriteEntries);
                if (EC_E_NOERROR != dwRetVal)
                {
                    LogError( (EC_T_CHAR*)"ERROR: emGetSlaveInpVarInfoEx() (Result = %s 0x%x)", ecatGetText(dwRetVal), dwRetVal );
                }
            }

            for(int i = 0; i < mySlaveInVarInfoNum; i++)
            {
                if(pProcessVarInfoIn[i].wIndex == DRV_OBJ_ERROR_CODE)  // 0x603F - Error Code
                {
                    My_Slave[MySlaves_num_tmp].myErrorCode  = (EC_T_WORD*)&(pbyPDIn[pProcessVarInfoIn[i].nBitOffs/8]);
                    LogMsg("Motrotech: MySlave[%d].myErrorCode = %d", MySlaves_num_tmp, My_Slave[MySlaves_num_tmp].myErrorCode );
                }                
                else if(pProcessVarInfoIn[i].wIndex == DRV_OBJ_STATUS_WORD)  // 0x6041 - StatusWord 
                {
                    My_Slave[MySlaves_num_tmp].myStatusWord  = (EC_T_WORD*)&(pbyPDIn[pProcessVarInfoIn[i].nBitOffs/8]);
                    LogMsg("Motrotech: MySlave[%d].myStatusWord = %d", MySlaves_num_tmp, My_Slave[MySlaves_num_tmp].myStatusWord );
                }
                else if(pProcessVarInfoIn[i].wIndex == DRV_OBJ_POSITION_ACTUAL_VALUE)  // 0x6064 - ActualPosition
                {
                    My_Slave[MySlaves_num_tmp].myActPosition  = (EC_T_DWORD*)&(pbyPDIn[pProcessVarInfoIn[i].nBitOffs/8]);
                    LogMsg("Motrotech: MySlave[%d].myActPosition = %d", MySlaves_num_tmp, My_Slave[MySlaves_num_tmp].myActPosition );
                }
                else if(pProcessVarInfoIn[i].wIndex == DRV_OBJ_VELOCITY_ACTUAL_VALUE)  // 0x606C - ActualVelocity
                {
                    My_Slave[MySlaves_num_tmp].myActVelocity  = (EC_T_DWORD*)&(pbyPDIn[pProcessVarInfoIn[i].nBitOffs/8]);
                    LogMsg("Motrotech: MySlave[%d].myActVelocity = %d", MySlaves_num_tmp, My_Slave[MySlaves_num_tmp].myActVelocity );
                }
                else if(pProcessVarInfoIn[i].wIndex == DRV_OBJ_TORQUE_ACTUAL_VALUE)  // 0x6077 - ActTorque
                {
                    My_Slave[MySlaves_num_tmp].myActTorque  = (EC_T_WORD*)&(pbyPDIn[pProcessVarInfoIn[i].nBitOffs/8]);
                    LogMsg("Motrotech: MySlave[%d].myActTorque = %d", MySlaves_num_tmp, My_Slave[MySlaves_num_tmp].myActTorque );
                }
                else if(pProcessVarInfoIn[i].wIndex == DRV_OBJ_FOLLOWING_ERROR)  // 0x60F4 - ActualFollowErr
                {
                    My_Slave[MySlaves_num_tmp].myActFollowErr  = (EC_T_DWORD*)&(pbyPDIn[pProcessVarInfoIn[i].nBitOffs/8]);
                    LogMsg("Motrotech: MySlave[%d].myActFollowErr = %d", MySlaves_num_tmp, My_Slave[MySlaves_num_tmp].myActFollowErr );
                }
//                else if(pProcessVarInfoIn[i].wIndex == DRV_OBJ_DIGITAL_INPUT)  // 0x6000 - Input 1
//                {
//                	  if(pProcessVarInfoOut[i].wSubIndex == DRV_OBJ_DIGITAL_INPUT_SUBINDEX_1)  // 0x6000/1 - Input subindex 1
//                    {
//                        My_Slave[MySlaves_num_tmp].myInput_1  = (EC_T_WORD*)&(pbyPDIn[pProcessVarInfoIn[i].nBitOffs/8]);
//                        LogMsg("Motrotech: MySlave[%d].myInput_1 = %d", MySlaves_num_tmp, My_Slave[MySlaves_num_tmp].myInput_1 );
//                    }
//                    else if(pProcessVarInfoOut[i].wSubIndex == DRV_OBJ_DIGITAL_INPUT_SUBINDEX_2)  // 0x6000/2 - Input subindex 2
//                    {
//                        My_Slave[MySlaves_num_tmp].myInput_2  = (EC_T_WORD*)&(pbyPDIn[pProcessVarInfoIn[i].nBitOffs/8]);
//                        LogMsg("Motrotech: MySlave[%d].myInput_2 = %d", MySlaves_num_tmp, My_Slave[MySlaves_num_tmp].myInput_2 );
//                    }
//                }
                else
                {
                	  //LogMsg("Motrotech: My_Slave[%d] Input undefine the index %d / usbindex %d", i, (pProcessVarInfoIn[i].wIndex, pProcessVarInfoIn[i].wSubIndex);
                }             
            }
        }
    }

    return EC_E_NOERROR;
}

/******************************************************************************
Function?myWorkpd
Description: Servo control demo code, re-code the acc and dec
Called By: myAppWorkpd
Return: EC_E_NOERROR
*******************************************************************************/
EC_T_VOID myWorkpd(
    CAtEmLogging*       poLog,          /* [in]  Logging instance */ 
    EC_T_INT            nVerbose,       /* [in]  Verbosity level */
    EC_T_BYTE*          pbyPDIn,        /* [in]  pointer to process data input buffer */
    EC_T_BYTE*          pbyPDOut        /* [in]  pointer to process data output buffer */
    )
{
    static eMoveState eMotionState[6] = {Homing, Homing,Homing,Homing,Homing,Homing};
    static EC_T_DWORD nWaitCounter1[6];
    static EC_T_DWORD eDistanceState[6];
    static EC_T_SDWORD Distance[6];
    static EC_T_SDWORD dwDriveDistance[6] = {1000,1500,2000,2500,3000,3500};
    static EC_T_SDWORD curDistance[6];

    EC_UNREFPARM(poLog);
    EC_UNREFPARM(nVerbose);
    EC_UNREFPARM(pbyPDIn);
    EC_UNREFPARM(pbyPDOut);

	for(EC_T_INT i = 0; i < MotorCount; i++)
	{
    if (My_Slave[i].myTargetPosition != NULL) 
    {
        My_Slave_Type *pDemoAxis = &My_Slave[i];
         
        if ((pDemoAxis != EC_NULL) && (My_Slave[i].wState == DRV_DEV_STATE_OP_ENABLED))
        {
            /* we are either in state "move setup", "moving" or "pause" */
            switch (eMotionState[i])
            {
            case EMoveSetup:
                {
                    double sign = /*i&1 ? -1.0 :*/ 1.0;

                    switch (eDistanceState[i])    /* do different distances */
                    {
                    default:    
                    case 0:
                        eDistanceState[i] = 0;
                        Distance[i] =  (EC_T_SDWORD)(sign * 2.0 * dwDriveDistance[i]);       /* + 2*Distance */
                        break;
                    case 1:
                        Distance[i] = (EC_T_SDWORD)( -sign * dwDriveDistance[i]);            /* - 1*Distance */
                        break;
                    case 2:
                        Distance[i] =  (EC_T_SDWORD)(-sign * (dwDriveDistance[i]/2));        /* - 1/2*Distance */
                        break;
                    case 3:
                        Distance[i] =  (EC_T_SDWORD)(-sign * (dwDriveDistance[i]/2));        /* - 1/2*Distance */
                        break;
                    }
#ifdef MTRACE
                    LogMsg("Motrotech: Distance Value: %x\n",Distance);
#endif /* define MTRACE */
                    eDistanceState[i]++;
                    eMotionState[i] = EMoving;
                }
            break;

            case EMoving:
            {
                curDistance[i] += Distance[i];
                EC_SETDWORD(My_Slave[i].myTargetPosition, curDistance[i]);
#ifdef MTRACE
                LogMsg("Motrotech: [%d] EMoving curDistance Value: %d\n",i,curDistance);
#endif /* define MTRACE */
                nWaitCounter1[i]++;
                if (nWaitCounter1[i] == 5000)
                {
                    eMotionState[i] = EPause;  
                }
            }
            break;

            case EPause:
                nWaitCounter1[i] = 0;
                eMotionState[i] = EMoveSetup;
                
            break;

            case Homing:
                curDistance[i] = EC_GETDWORD(My_Slave[i].myActPosition);
#ifdef MTRACE
                LogMsg("Motrotech: Homing CurDistance: %d\n", curDistance);
#endif /* define MTRACE */                 
                if (((curDistance[i] > 0) && (curDistance[i] < 500)) || ((curDistance[i] < 0) && (curDistance[i] > -500)))
                {
                    eMotionState[i] = EMoveSetup;
                }
                else
                {
                    if (curDistance[i] > 0) 
                        curDistance[i] -= 500;
                    else
                        curDistance[i] += 500;
                }
                EC_SETDWORD(My_Slave[i].myTargetPosition, curDistance[i]);

            break;
            default:
            {
                OsDbgAssert(0);
            }
            break;
            }
        }
    } /* loop through axis list */
	}
}

/******************************************************************************
Function: Process_Switch
Description: send the command for COMMAND_STOP & COMMAND_START
Called By: DCDemo
Return: EC_E_NOERROR
*******************************************************************************/
EC_T_VOID Process_Switch(EC_T_INT AxisNum,
     EC_T_DWORD command
) 
{
    if (AxisNum < MotorCount)
    {
        switch (command)
        {
        case COMMAND_START:
            S_ProcessState[AxisNum] = COMMAND_START;
            break;
        case COMMAND_STOP:
            S_ProcessState[AxisNum] = COMMAND_STOP;
            break;
				}
    }
}

EC_T_DWORD Process_SetModOperation(
     CAtEmLogging*     poLog
   , EC_T_INT          nVerbose
   , EC_T_WORD         ControlMode
   )
{
    EC_UNREFPARM(poLog);
    EC_UNREFPARM(nVerbose);

	static EC_T_DWORD* dwCommand = S_ProcessState;

    for (EC_T_INT mIndex = 0; mIndex < MotorCount; mIndex++) 
    {
        if (My_Slave[mIndex].byModesOfOperation != EC_NULL)
        {
            if ( dwCommand[mIndex] == COMMAND_START )
            {
                if (ControlMode == OPMODE_CSP) 
                {
				    My_Slave[mIndex].byModesOfOperation = DRV_MODE_OP_CSP;
                }
                else if (ControlMode == OPMODE_CSV) 
                {
                    My_Slave[mIndex].byModesOfOperation = DRV_MODE_OP_CSV;
                } 
                else if (ControlMode == OPMODE_CST)
                {
                    My_Slave[mIndex].byModesOfOperation = DRV_MODE_OP_CST;
                }
            }

            EC_SETWORD(My_Slave[mIndex].myModeOfOperation, My_Slave[mIndex].byModesOfOperation);
		}
	}

	return EC_E_NOERROR;
}

/******************************************************************************
Function:Process_Commands
Description: instand for Demo_run and Demo_stop
Called By: DCDemo
Return: EC_E_NOERROR
*******************************************************************************/
EC_T_DWORD Process_Commands(
     CAtEmLogging*     poLog
   , EC_T_INT          nVerbose
) 
{
    static EC_T_DWORD  S_dwStatus = 0;
	  static EC_T_WORD   wControl[6];
    static EC_T_SDWORD curDistance[6];
	  static EC_T_DWORD* dwCommand = S_ProcessState;

    EC_UNREFPARM(poLog);
    EC_UNREFPARM(nVerbose);
    
    for (EC_T_INT mIndex = 0; mIndex < MotorCount; mIndex++) 
    {
        if (My_Slave[mIndex].myStatusWord != EC_NULL)
        {
            S_dwStatus = EC_GETWORD(My_Slave[mIndex].myStatusWord);
            
			if (dwCommand[mIndex] == COMMAND_START) 
            {
                if ( (S_dwStatus & 0x8) == DRV_STAT_FAULT ) 
                {
                    wControl[mIndex] = DRV_CRTL_FAULT_RESET;
                } 
                else 
                {
                    switch (My_Slave[mIndex].wState) 
                    {
                    case DRV_DEV_STATE_NOT_READY:						
                        curDistance[mIndex] = EC_GETDWORD(My_Slave[mIndex].myActPosition);
                        EC_SETDWORD(My_Slave[mIndex].myTargetPosition, curDistance[mIndex]);

                        if((S_dwStatus & 0x4F) == 0x00 
                            || (S_dwStatus & 0x4F) == DRV_STAT_SWITCH_ON_DIS 
                            || (S_dwStatus & 0x4F) == (DRV_STAT_SWITCH_ON_DIS | DRV_STAT_QUICK_STOP))
                        {
                            LogMsg("Slave[%d] To Switch On Disabled %04x\n", mIndex, S_dwStatus);
                            wControl[mIndex] = DRV_CTRL_CMD_SHUTDOWN;
                            My_Slave[mIndex].wState = DRV_DEV_STATE_SWITCHON_DIS;
                        } 
                        else if (S_dwStatus & DRV_STAT_FAULT) 
                        {
                           LogMsg("Slave[%d] To Malfunction %04x\n", mIndex, S_dwStatus);
                           wControl[mIndex] = DRV_CRTL_FAULT_RESET;
                           My_Slave[mIndex].wState = DRV_DEV_STATE_MALFCT_REACTION;
                        } 
						else if ((S_dwStatus & DRV_STAT_RDY_SWITCH_ON) == DRV_STAT_RDY_SWITCH_ON)
						{
                           My_Slave[mIndex].wState = DRV_DEV_STATE_SWITCHON_DIS;
						}
                        break;
                    case DRV_DEV_STATE_SWITCHON_DIS:    
                        if((S_dwStatus & 0x4F) == DRV_STAT_SWITCH_ON_DIS)
                        {
                            LogMsg("Slave[%d] To Ready to Switch On %04x\n", mIndex, S_dwStatus);
                            wControl[mIndex] = DRV_CTRL_CMD_SHUTDOWN;
                            My_Slave[mIndex].wState = DRV_DEV_STATE_READY_TO_SWITCHON;
                        } 
						else if((S_dwStatus & 0x4F) == DRV_STAT_RDY_SWITCH_ON)
                        {
                            My_Slave[mIndex].wState = DRV_DEV_STATE_READY_TO_SWITCHON;
                        }
                        else if((S_dwStatus & 0x69) == (DRV_STAT_QUICK_STOP | DRV_STAT_RDY_SWITCH_ON))
                        {
                            My_Slave[mIndex].wState = DRV_DEV_STATE_SWITCHED_ON;
                        }
                        else if(S_dwStatus & DRV_STAT_FAULT)
                        {
                            My_Slave[mIndex].wState = DRV_DEV_STATE_MALFUNCTION;
                        }
                        break;
                    case DRV_DEV_STATE_READY_TO_SWITCHON:   
                        if((S_dwStatus & 0x6F) == (DRV_STAT_QUICK_STOP | DRV_STAT_RDY_SWITCH_ON)) 
                        {
                            LogMsg("Slave[%d] To Switched On %04x\n", mIndex, S_dwStatus);
                            wControl[mIndex] = DRV_CTRL_CMD_SWITCHON;
                            My_Slave[mIndex].wState = DRV_DEV_STATE_SWITCHED_ON;
                        }
                        break;
                    case DRV_DEV_STATE_SWITCHED_ON:
                        if((S_dwStatus & 0x6F) == (DRV_STAT_QUICK_STOP | DRV_STAT_SWITCHED_ON | DRV_STAT_RDY_SWITCH_ON)) 
                        {
                            LogMsg("Slave[%d] To Operation Enabled %04x\n", mIndex, S_dwStatus);
                            wControl[mIndex] = DRV_CTRL_CMD_ENA_OPERATION;
                            My_Slave[mIndex].wState = DRV_DEV_STATE_OP_ENABLED;
                        }
                        break;
                    case DRV_DEV_STATE_OP_ENABLED:
                        if((S_dwStatus & 0x7F) == (DRV_STAT_QUICK_STOP | DRV_STAT_VOLTAGE_ENABLED | DRV_STAT_OP_ENABLED | DRV_STAT_SWITCHED_ON | DRV_STAT_RDY_SWITCH_ON)) 
                        {
							//LogMsg("Slave[%d] In Operation Enabled %04x\n", mIndex, S_dwStatus);
                        }  
                        break;
                    case DRV_DEV_STATE_MALFCT_REACTION:               
                        LogMsg("Slave[%d] To No Ready %04x\n", mIndex, S_dwStatus);
                        wControl[mIndex] = DRV_CRTL_FAULT_RESET;
                        My_Slave[mIndex].wState = DRV_DEV_STATE_NOT_READY;
                        break;
                    default:
                        LogMsg("Motrotech: State: %d, Status: %04x\n", My_Slave[mIndex].wState, S_dwStatus);
                        break;
                    }
                }
            } 
            else if (dwCommand[mIndex] == COMMAND_STOP) 
            {
				LogMsg("Slave[%d] To Switch On Disabled %04x\n", mIndex, S_dwStatus);
				
                wControl[mIndex] = DRV_CTRL_CMD_SHUTDOWN;              //0x00;
                My_Slave[mIndex].wState = DRV_DEV_STATE_NOT_READY;     //state_ready_to_switchon;
				dwCommand[mIndex] = COMMAND_NONE;
			} 

            if (My_Slave[mIndex].myControlWord != EC_NULL)
            {
                EC_SETWORD(My_Slave[mIndex].myControlWord, wControl[mIndex]);
            }
        } 
        else 
        {
            //LogMsg("Wrong State: %d\n", My_Slave[mIndex].myStatusWord);
        }
    }
    return EC_E_NOERROR;
}

/*-----------------------------end----------------------------------*/
